import numpy as np 

class EventTriggeredASTLFController:
#事件触发自适应控制器
#输入：横向偏差L_os和航向偏差theta_os

    #初始化控制器参数
    def __init__(self, c1:float = 0.3, c2:float = 1,
                 epsilon:float = -0.42, sigma:float = 0.4,
                 gamma: float = 0.2, mu0: float = 0.02,
                 mu1: float = 0.02, c: float = 0.18,
                 beta1_min: float = 2.5, beta1_initial: float = 2.8,
                 dt: float=0.01):
                
            self.c1 = c1
            self.c2 = c2
            #滑模面参数

            self.dt = dt
            #系统参数

            self.epsilon = epsilon
            self.sigma = sigma
            self.gamma = gamma
            #事件触发参数

            self.mu0 = mu0
            self.mu1 = mu1
            self.c = c
            self.beta1_min = beta1_min
            #自适应参数

            self.beta1 = beta1_initial
            self.beta2 = 2 * c * (1 + epsilon) * beta1_initial
            #自适应状态变量

            self.s_tk = 0.0                     # 上次触发时的滑模变量值
            self.u0 = 0.0                       # 积分项
            #事件触发相关状态

    #计算滑模面值
    def sliding_surface(self, L_os:float, theta_os:float)->float:
            return self.c1 * theta_os * 180 / np.pi + self.c2 * L_os
    
    #检测是否触发
    def event_trigger_condition(self, s:float)->bool:
           
           #计算误差e
           e = np.sign(self.s_tk) * np.abs(self.s_tk) ** (2 + 2*self.epsilon) - np.sign(s) * np.abs(s) ** (2 + 2*self.epsilon)
           
           #事件触发阙值
           threshold = self.sigma * (np.abs(self.s_tk) ** (1 + self.epsilon) + self.gamma)

           #检查触发条件
           return np.abs(e) >= threshold
    
    #更新自适应参数
    def update_adaptive_parameters(self , s : float):
           omega = self.sigma * (np.abs(self.s_tk) ** (1 + self.epsilon) + self.gamma)
           if self.beta1 > self.beta1_min:
                sign_val = np.sign(np.abs(s) ** (2 + 2 * self.epsilon) -omega)
                beta1_dot = self.mu1 * sign_val
           else:
                beta1_dot = self.mu0
           self.beta1 += beta1_dot * self.dt
           self.beta2 = 2 * self.c * (1 + self.epsilon) * self.beta1

    #计算控制输入
    def compute_control(self,s:float)->np.double:
           
           #检查是否触发
           if self.event_trigger_condition(s):
                self.s_tk = s
                self.u0 = 0.0                       # 重置积分项

           u0_dot = -self.beta2 * np.abs(self.s_tk) ** (1 + 2 * self.epsilon)
           self.u0 += u0_dot * self.dt

           u = 1.6 * (-self.beta1 * np.sign(self.s_tk) * np.abs(self.s_tk) ** (1+self.epsilon) + self.u0)

           return u
    
    #将 u 转化为实际转向角 delta_CAN
    def get_steering_angle(self,u:float)->np.single:
          delta_CAN = (u * 25)
          if np.abs(delta_CAN) > 600:
            delta_CAN = np.sign(delta_CAN) * 600
          return delta_CAN     

    #控制器单步更新
    def step(self,L_os:float, theta_os:float)->np.single:
          
          s=self.sliding_surface(L_os,theta_os)                       #计算滑模量的值
          
          self.update_adaptive_parameters(s)                          #更新自适应参数

          u = self.compute_control(s)                                 #计算控制输入

          delta_CAN = self.get_steering_angle(u)                      #计算实际转向角

          return delta_CAN
          